#pragma once
#ifndef SAMPLER_H
#define SAMPLER_H

#include "basic_typedef.h"
#include "CollisionDetector.h"

class Sampler
{
public:
	
	////////////////////////
	// CTORS
	////////////////////////
	/*	Parameters:
			radius	- radius of robot
			Room	- describes the boundary of the scenario
			CollisionDetector	- Collision detection black box
	*/

	Sampler(double radius, Room room, CollisionDetector *col, double seed = -1) : 
	  m_radius(radius), m_col(col), m_room(room)
	{	
		tl = m_room.first;
		br = m_room.second;
		xmin = tl.x() + m_radius;
		xmax = br.x() - m_radius;
		ymax = br.y() - m_radius;
		ymin = tl.y() + m_radius;
		lenx = xmax - xmin;
		leny = ymax - ymin;

		if (seed == -1)
			seed = time(NULL);
		srand ( seed );
	}

	~Sampler(void)
    {

    }

	////////////////////////
	// Queries
	////////////////////////
	
	/*	Returns a point that represents a valid (free) 
		configuration of the robot. */
	Point_2 generate_sample()
	{
		{
			while(true)
			{
				// sample a point in the configuration space
				Point_2 p = generate_sample_no_obstacles();

				// validate sample using a collision detector
				if (m_col->valid_conf(p))
					return p;
			}
		}
	}

private:

	/*	Samples a point in the configuration space of the robot
		defined by the bounding box of the scenario.
		This point can be valid or invalid.	*/
	Point_2 generate_sample_no_obstacles()
	{
		double randx = xmin + lenx * ((double)rand())/((double)RAND_MAX);
		double randy = ymin + leny * ((double)rand())/((double)RAND_MAX);

		Point_2 p(randx,randy);

		return p;
	}

	////////////////////////
	// Data Members
	////////////////////////
	double		m_radius;	// radius of robots
		
	Room		m_room;		// Represents the boundaries of the scenario
	CollisionDetector	*m_col;	// Collision detector

	// room variables
	Point_2 tl;
	Point_2 br;
	double xmin;
	double xmax;
	double ymin;
	double ymax;
	double lenx;
	double leny;
};
#endif